﻿using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;

namespace Patyk.MapEditor.Geometry
{
    public class InterpolatedCurve
    {
        private float _dt = 0.01f;

        private readonly List<Vector3> _nodes;
        private readonly List<Vector3> _curve;
        public Color Color = Color.Red;
        public List<Vector3> Curve { get { return _curve; } }

        public InterpolatedCurve()
        {
            _nodes = new List<Vector3>();
            _curve = new List<Vector3>();
        }

        public InterpolatedCurve(List<Vector2> nodes)
        {
            _nodes = new List<Vector3>();
            _curve = new List<Vector3>();

            _nodes = nodes.Select(x => new Vector3(x.X, x.Y, 0)).ToList();

            InitializeCurve();
        }

        private void InitializeCurve()
        {
            if (_nodes.Count < 4) return;

            int n = _nodes.Count - 1;
            int alfaLength = n - 1; int diagLength = n - 1; int betaLength = n - 1;
            int hLength = n; int xparamLength = n + 1; int yparamLength = n + 1;

            var alfa = new float[alfaLength];
            var diag = new float[diagLength];
            var beta = new float[betaLength];
            var h = new float[hLength];
            var xparam = new float[xparamLength];
            var yparam = new float[yparamLength];

            float length = 0;
            for (int i = 0; i < n; ++i)
                length += Vector3.Distance(_nodes[i], _nodes[i + 1]);

            xparam[0] = 0.0f;
            for (int i = 0; i < n; ++i)
                xparam[i + 1] = xparam[i] + Vector3.Distance(_nodes[i], _nodes[i + 1])/length;

            for (int i = 0; i < n; ++i)
                h[i] = xparam[i + 1] - xparam[i];

            alfa[0] = beta[betaLength - 1] = 0.0f;
            for (int i = 1; i < n; i++)
            {
                if (i > 1)
                    alfa[i - 1] = h[i - 1] / (h[i - 1] + h[i]);
                if (i < n - 1)
                    beta[i - 1] = h[i] / (h[i - 1] + h[i]);
            }

            for (int i = 0; i < diagLength; ++i)
                diag[i] = 2;

            for (int i = 0; i < _nodes.Count; ++i)
                yparam[i] = _nodes[i].X;

            float[] x = Resolve(beta, diag, alfa, h, xparam, yparam, n);

            for (int i = 0; i < _nodes.Count; ++i)
                yparam[i] = _nodes[i].Y;

            float[] y = Resolve(beta, diag, alfa, h, xparam, yparam, n);

            for (int i = 0; i < _nodes.Count; ++i)
                yparam[i] = _nodes[i].Z;

            float[] z = Resolve(beta, diag, alfa, h, xparam, yparam, n);

            for(int i = 0; i < x.Length; ++i)
                _curve.Add(new Vector3(x[i], y[i], z[i]));
        }

        private float[] Resolve(float[] beta, float[] diag, float[] alfa, float[] h, float[] xparam, float[] y, int n)
        {
            var a = new float[n];
            var b = new float[n];
            var c = new float[n + 1];
            var d = new float[n];
            var betacopy = new float[n - 1];
            var R = new float[n - 1];
            var finalResult = new List<float>();

            for (int i = 1; i < n; ++i)
                R[i - 1] = 3.0f*((y[i + 1] - y[i])/h[i] - (y[i] - y[i - 1])/h[i - 1])/(h[i - 1] + h[i]);

            for (int i = 0; i < betacopy.Length; ++i)
                betacopy[i] = beta[i];

            var result = ResolveEquations(alfa, diag, betacopy, R);

            c[0] = 0.0f;
            c[c.Length - 1] = 0.0f;
            for (int i = 1; i < n; ++i)
                c[i] = result[i - 1];

            for (int i = 0; i < n; ++i)
            {
                a[i] = y[i];
                d[i] = (c[i + 1] - c[i])/(3.0f*h[i]);
                b[i] = ((y[i + 1] - y[i])/h[i]) - (h[i]/3.0f)*(c[i + 1] + 2.0f*c[i]);
            }

            float x = 0.0f;
            for (int j = 0; j < n; ++j)
            {
                while (xparam[j] <= x && x < xparam[j + 1])
                {
                    x += _dt;
                    float t = x - xparam[j];
                    finalResult.Add(a[j] + b[j]*t + c[j]*t*t + d[j]*t*t*t);
                }
            }
            return finalResult.ToArray();
        }

        /// <summary>
        /// Rozwiązanie układu równań (Ax = d) w macierzą trójdiagonalną w czasie liniowym
        /// A - macierz trójdiagonalna
        /// </summary>
        /// <param name="a">wektor wartości pod główną diagonalą</param>
        /// <param name="b">główna diagonala macierzy A</param>
        /// <param name="c">wektor wartości nad główną diagonalą</param>
        /// <param name="d">wektor wyrazów wolnych</param>
        /// <returns>wektor niewiadomych</returns>
        static float[] ResolveEquations(float[] a, float[] b, float[] c, float[] d)
        {
            /* Warning: will modify c and d! */
            int n = d.Length; /* All arrays should be the same length */
            var x = new float[n];

            /* Modify the coefficients. */
            c[0] /= b[0]; /* Division by zero risk. */
            d[0] /= b[0]; /* Division by zero would imply a singular matrix. */
            for (int i = 1; i < n; i++)
            {
                float id = 1.0f / (b[i] - c[i - 1] * a[i]);
                c[i] *= id; /* This calculation during the last iteration is redundant. */
                d[i] = (d[i] - d[i - 1] * a[i]) * id;
            }

            /* Now back substitute. */
            x[n - 1] = d[n - 1];
            for (int i = n - 2; i >= 0; i--)
                x[i] = d[i] - c[i] * x[i + 1];

            return x;
        }
    }
}
